#目前完成了电机的初始化和电源的检测，写了PID的控制算法
#设备使用的是空心杯电机，所以无需电调即可驱动，但是需要使用PWM来控制电机的转速
#电机的转速由PID算法实时计算后控制
import machine,socket,network
from machine import Pin, I2C, ADC, PWM
import time
import _thread

i2c=I2C(sda=Pin(34),scl=Pin(35),freq=1000000)
i2c.scan()
#初始化I2C
led_r=Pin(22,Pin.OUT)
#红色LED主要用于显示错误信息，一般为关闭
led_r.value(1)
led=PWM(2,Pin.OUT)
#板载正面蓝色led，显示系统状态
led.freq(1000)
led.duty(1023)
led_b=PWM(Pin(15,Pin.OUT))
#底部蓝色led，飞行指示灯
led_b.freq(10)
led_b.duty(1023)
power_ADC_pin = Pin(13, Pin.IN)
#读取电池电压
power_ADC = ADC(power_ADC_pin)
PWM_PIN1 =PWM(0,Pin.OUT)
PWM_PIN2 =PWM(4,Pin.OUT)
PWM_PIN3 =PWM(16,Pin.OUT)
PWM_PIN4 =PWM(17,Pin.OUT)
#电机控制引脚
PWM_PIN1.freq(1000)
PWM_PIN2.freq(1000)
PWM_PIN3.freq(1000)
PWM_PIN4.freq(1000)
PWM_PIN1.duty(10)
PWM_PIN2.duty(10)
PWM_PIN3.duty(10)
PWM_PIN4.duty(10)
time.sleep(5)
PWM_PIN1.duty(0)
PWM_PIN2.duty(0)
PWM_PIN3.duty(0)
PWM_PIN4.duty(0)
print("UAV PWM READY")
led.freq(5)
led.duty(1023)
led_r.value(0)
led_b.freq(5)
led_b.duty(1023)
#初始化电机
def power_ADC():
    return power_ADC.read_u16()
#读取电池电压
def pid_control(current_state, desired_state, dt):
    """
    PID控制算法

    :param current_state: 当前状态
    :param desired_state: 期望状态
    :param dt: 时间间隔
    :param kp: 比例增益
    :param ki: 积分增益
    :param kd: 微分增益
    :return: 控制器输出
    """
    kp=1.0
    ki=0.1
    kd=0.05
    # 静态变量用于保存上一次的误差和积分
    if not hasattr(pid_control, "last_error"):
        pid_control.last_error = 0.0
    if not hasattr(pid_control, "integral"):
        pid_control.integral = 0.0

    error = desired_state - current_state

    # Proportional term
    P_out = kp * error

    # Integral term
    pid_control.integral += error * dt
    I_out = ki * pid_control.integral

    # Derivative term
    derivative = (error - pid_control.last_error) / dt
    D_out = kd * derivative

    # Calculate total output
    output = P_out + I_out + D_out

    # Save error for next update
    pid_control.last_error = error

    return output 
#PID控制算法
def wifi_link():
    sta_if = network.WLAN(network.STA_IF)
    sta_if.active(True)
    sta_if.connect('bee_net','1234567890')
    if sta_if.isconnected():
        return True
    else :
        return False
#wifi连接
def net_link(server_ip, server_port):
    client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    client_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    client_socket.connect((server_ip, server_port))
    print("Connected to server:", server_ip, server_port)
#网络连接
def tcp_server(server_ip, server_port):
    # 创建一个 socket 对象
    server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    
    # 绑定到本地 IP 和端口
    server_socket.bind((server_ip, server_port))
    server_socket.listen(1)
    print("Server started on", server_ip, "port", server_port)
#socket
def I2c_gps():
    return i2c.readfrom_mem(0x42, 0x00, 1)
#GPS数据读取
